#include "iena_lib.h"

/* 简单的内存拷贝函数 - 替代memcpy */
static void iena_memcpy(void *dst, const void *src, size_t len) {
    uint8_t *d = (uint8_t *)dst;
    const uint8_t *s = (const uint8_t *)src;
    while (len--) {
        *d++ = *s++;
    }
}

/* 错误代码定义 */
#define IENA_OK                0
#define IENA_ERR_NULL_POINTER  -1
#define IENA_ERR_BUFFER_FULL   -2
#define IENA_ERR_INVALID_PARAM -3
#define IENA_ERR_NOT_STARTED   -4
#define IENA_ERR_ALREADY_DONE  -5

/* 内联函数：将16位值写入缓冲区 (大端序) */
static inline void write_u16_be(uint8_t *buffer, uint16_t value) {
    buffer[0] = (uint8_t)(value >> 8);
    buffer[1] = (uint8_t)(value & 0xFF);
}

/* 内联函数：从缓冲区读取16位值 (大端序) */
static inline uint16_t read_u16_be(const uint8_t *buffer) {
    return ((uint16_t)buffer[0] << 8) | buffer[1];
}

/* 内联函数：检查缓冲区剩余空间 */
static inline int check_buffer_space(const iena_builder_t *builder, uint16_t needed) {
    return (builder->current_pos + needed <= builder->buffer_size) ? IENA_OK : IENA_ERR_BUFFER_FULL;
}
// 专门用于16位字数组的字节序转换拷贝
static void iena_copy_words_be(uint8_t *dst, const uint16_t *src, uint16_t word_count) {
    for (uint16_t i = 0; i < word_count; i++) {
        write_u16_be(&dst[i * 2], src[i]);
    }
}
/**
 * 初始化IENA包构建器
 */
int iena_builder_init(iena_builder_t *builder,
                      uint8_t *buffer,
                      uint16_t buffer_size,
                      iena_key_config_t *key_config) {
    if (!builder || !buffer || !key_config) {
        return IENA_ERR_NULL_POINTER;
    }

    if (buffer_size < IENA_MIN_PACKET_SIZE) {
        return IENA_ERR_INVALID_PARAM;
    }

    builder->buffer = buffer;
    builder->buffer_size = buffer_size;
    builder->current_pos = 0;
    builder->key_config = key_config;
    builder->is_header_written = 0;

    return IENA_OK;
}

/**
 * 开始构建IENA包 (写入包头)
 */
int iena_begin_packet(iena_builder_t *builder,
                      iena_timestamp_func_t get_timestamp,
                      iena_status_func_t get_status) {
    if (!builder || !get_timestamp || !get_status) {
        return IENA_ERR_NULL_POINTER;
    }

    if (builder->is_header_written) {
        return IENA_ERR_ALREADY_DONE;
    }

    int ret = check_buffer_space(builder, IENA_HEADER_SIZE);
    if (ret != IENA_OK) return ret;

    uint8_t *buf = builder->buffer;

    // 写入IENA Key (2字节)
    write_u16_be(&buf[0], builder->key_config->key_id);

    // Size字段暂时填0，等最后完成时再更新 (2字节)
    write_u16_be(&buf[2], 0);

    // 写入时间戳 (6字节)
    uint64_t timestamp = get_timestamp();
    iena_timestamp_to_bytes(timestamp, &buf[4]);

    // 写入Key Status (1字节)
    buf[10] = iena_make_key_status(builder->key_config->packet_type,
                                   builder->key_config->word_length);

    // 写入Status (1字节)
    buf[11] = get_status();

    // 写入序列号并递增 (2字节)
    write_u16_be(&buf[12], builder->key_config->seq_num++);

    builder->current_pos = IENA_HEADER_SIZE;
    builder->is_header_written = 1;

    return IENA_OK;
}


/**
 * P型包 - 添加一组参数数据 (修复版本)
 */
int iena_add_p_params(iena_builder_t *builder,
                      const void *param_data,
                      uint8_t param_count) {
    if (!builder || !param_data) {
        return IENA_ERR_NULL_POINTER;
    }

    if (!builder->is_header_written) {
        return IENA_ERR_NOT_STARTED;
    }

    if (builder->key_config->packet_type != IENA_TYPE_P) {
        return IENA_ERR_INVALID_PARAM;
    }

    if (param_count != builder->key_config->param_count) {
        return IENA_ERR_INVALID_PARAM;
    }

    uint16_t param_size = builder->key_config->word_length * 2; // 字节数
    uint16_t total_size = param_count * param_size;

    int ret = check_buffer_space(builder, total_size);
    if (ret != IENA_OK) return ret;

    // ✅ 使用字节序转换拷贝 (P型包需要转换)
    uint16_t total_words = total_size / 2;
    iena_copy_words_be(&builder->buffer[builder->current_pos],
                       (const uint16_t *)param_data,
                       total_words);

    builder->current_pos += total_size;
    return IENA_OK;
}

/**
 * M型包 - 添加一个参数块 (保持原样)
 */
int iena_add_m_param(iena_builder_t *builder,
                     uint16_t param_id,
                     uint16_t offset,
                     const void *data,
                     uint16_t data_len) {
    if (!builder || !data) {
        return IENA_ERR_NULL_POINTER;
    }

    if (!builder->is_header_written) {
        return IENA_ERR_NOT_STARTED;
    }

    if ((builder->key_config->packet_type & 0x30) != 0x30) {
        return IENA_ERR_INVALID_PARAM;
    }

    if (data_len > 65484) {
        return IENA_ERR_INVALID_PARAM;
    }

    uint16_t padding = (data_len & 1) ? 1 : 0;
    uint16_t total_size = 6 + data_len + padding;

    int ret = check_buffer_space(builder, total_size);
    if (ret != IENA_OK) return ret;

    uint8_t *buf = &builder->buffer[builder->current_pos];

    // ✅ 参数ID和Offset需要大端序转换
    write_u16_be(&buf[0], param_id);
    write_u16_be(&buf[2], offset);
    write_u16_be(&buf[4], data_len);

    // ✅ Dataset使用原始拷贝 (M型包的数据不需要字节序转换)
    iena_memcpy(&buf[6], data, data_len);

    if (padding) {
        buf[6 + data_len] = 0;
    }

    builder->current_pos += total_size;
    return IENA_OK;
}

/**
 * Q型包 - 添加一个参数块 (修改类似M型)
 */
int iena_add_q_param(iena_builder_t *builder,
                     uint16_t param_id,
                     const void *data,
                     uint16_t data_len) {
    if (!builder || !data) {
        return IENA_ERR_NULL_POINTER;
    }

    if (!builder->is_header_written) {
        return IENA_ERR_NOT_STARTED;
    }

    if ((builder->key_config->packet_type & 0x30) != 0x20) {
        return IENA_ERR_INVALID_PARAM;
    }

    if (data_len > 65484) {
        return IENA_ERR_INVALID_PARAM;
    }

    uint16_t padding = (data_len & 1) ? 1 : 0;
    uint16_t total_size = 4 + data_len + padding;

    int ret = check_buffer_space(builder, total_size);
    if (ret != IENA_OK) return ret;

    uint8_t *buf = &builder->buffer[builder->current_pos];

    // ✅ 参数ID和Length需要大端序转换
    write_u16_be(&buf[0], param_id);
    write_u16_be(&buf[2], data_len);

    // ✅ Dataset使用原始拷贝 (Q型包的数据不需要字节序转换)
    iena_memcpy(&buf[4], data, data_len);

    if (padding) {
        buf[4 + data_len] = 0;
    }

    builder->current_pos += total_size;
    return IENA_OK;
}

/**
 * D型包 - 添加一个参数块 (修复版本)
 */
int iena_add_d_param(iena_builder_t *builder,
                     uint16_t param_id,
                     uint16_t delay,
                     const void *data) {
    if (!builder || !data) {
        return IENA_ERR_NULL_POINTER;
    }

    if (!builder->is_header_written) {
        return IENA_ERR_NOT_STARTED;
    }

    if ((builder->key_config->packet_type & 0x30) != 0x10) {
        return IENA_ERR_INVALID_PARAM;
    }

    uint16_t data_size = builder->key_config->word_length * 2;
    uint16_t total_size = 4 + data_size;

    int ret = check_buffer_space(builder, total_size);
    if (ret != IENA_OK) return ret;

    uint8_t *buf = &builder->buffer[builder->current_pos];

    // ✅ 参数ID和Delay需要大端序转换
    write_u16_be(&buf[0], param_id);
    write_u16_be(&buf[2], delay);

    // ✅ Data使用字节序转换拷贝 (D型包的数据需要转换)
    uint16_t word_count = data_size / 2;
    iena_copy_words_be(&buf[4], (const uint16_t *)data, word_count);

    builder->current_pos += total_size;
    return IENA_OK;
}

/**
 * N型包 - 添加一个参数块 (修复版本)
 */
int iena_add_n_param(iena_builder_t *builder,
                     uint16_t param_id,
                     const void *data) {
    if (!builder || !data) {
        return IENA_ERR_NULL_POINTER;
    }

    if (!builder->is_header_written) {
        return IENA_ERR_NOT_STARTED;
    }

    if ((builder->key_config->packet_type & 0xB0) != 0x00) {
        return IENA_ERR_INVALID_PARAM;
    }

    uint16_t data_size = builder->key_config->word_length * 2;
    uint16_t total_size = 2 + data_size;

    int ret = check_buffer_space(builder, total_size);
    if (ret != IENA_OK) return ret;

    uint8_t *buf = &builder->buffer[builder->current_pos];

    // ✅ 参数ID需要大端序转换
    write_u16_be(&buf[0], param_id);

    // ✅ Data使用字节序转换拷贝 (N型包的数据需要转换)
    uint16_t word_count = data_size / 2;
    iena_copy_words_be(&buf[2], (const uint16_t *)data, word_count);

    builder->current_pos += total_size;
    return IENA_OK;
}


/**
 * 完成IENA包构建
 */
uint16_t iena_finalize_packet(iena_builder_t *builder) {
    if (!builder || !builder->is_header_written) {
        return 0;
    }

    // 检查包尾空间
    if (check_buffer_space(builder, IENA_TRAILER_SIZE) != IENA_OK) {
        return 0;
    }

    // 写入包尾
    write_u16_be(&builder->buffer[builder->current_pos], IENA_END_FIELD);
    builder->current_pos += IENA_TRAILER_SIZE;

    // 更新包头中的Size字段
    write_u16_be(&builder->buffer[2], builder->current_pos);

    return builder->current_pos;
}

/**
 * 重置构建器
 */
void iena_reset_builder(iena_builder_t *builder) {
    if (builder) {
        builder->current_pos = 0;
        builder->is_header_written = 0;
    }
}

/**
 * 创建键状态字节
 */
uint8_t iena_make_key_status(iena_packet_type_t packet_type,
                             iena_word_length_t word_length) {
    uint8_t key_status = packet_type & 0xB0; // 保留Bit7,5,4

    // 对于P,D,N型包，设置字长度位 (Bit2-0)
    if ((packet_type & 0x20) == 0) { // Bit5=0 表示定长
        key_status |= (word_length & 0x07);
    }

    return key_status;
}

/**
 * 48位时间戳转换为字节数组
 */
void iena_timestamp_to_bytes(uint64_t timestamp, uint8_t time_bytes[6]) {
    // 只取低48位，按大端序存储
    time_bytes[0] = (uint8_t)(timestamp >> 40);
    time_bytes[1] = (uint8_t)(timestamp >> 32);
    time_bytes[2] = (uint8_t)(timestamp >> 24);
    time_bytes[3] = (uint8_t)(timestamp >> 16);
    time_bytes[4] = (uint8_t)(timestamp >> 8);
    time_bytes[5] = (uint8_t)(timestamp);
}

/**
 * 字节数组转换为48位时间戳
 */
uint64_t iena_bytes_to_timestamp(const uint8_t time_bytes[6]) {
    return ((uint64_t)time_bytes[0] << 40) |
           ((uint64_t)time_bytes[1] << 32) |
           ((uint64_t)time_bytes[2] << 24) |
           ((uint64_t)time_bytes[3] << 16) |
           ((uint64_t)time_bytes[4] << 8)  |
           ((uint64_t)time_bytes[5]);
}






/* P型包数据表格 */
static void print_p_data_table(const uint8_t *data, uint16_t data_len, uint8_t key_status) {
    uint8_t param_bytes = (key_status & 0x07) * 2;
    uint16_t param_count = data_len / param_bytes;

    printf("P型包 - 定位参数 (Positional)\n");
    printf("┌─────────────┬────────────┬──────────────┬─────────────────────────┬──────────────────┐\n");
    printf("│ 参数序号    │ 长度       │ 值 (十六进制) │ 值 (十进制)             │ 内存地址         │\n");
    printf("├─────────────┼────────────┼──────────────┼─────────────────────────┼──────────────────┤\n");

    for (uint16_t i = 0; i < param_count && i < 10; i++) {  // 最多显示10个
        const uint8_t *param = &data[i * param_bytes];
        printf("│ Parameter %-2u │ %u 字节    │ ", i, param_bytes);

        // 打印十六进制值
        for (int j = 0; j < param_bytes; j++) {
            printf("%02X", param[j]);
        }
        for (int j = param_bytes; j < 6; j++) printf(" ");  // 对齐
        printf(" │ ");

        // 打印十进制值
        if (param_bytes == 2) {
            uint16_t val = (param[0] << 8) | param[1];
            printf("%-23u", val);
        } else if (param_bytes == 4) {
            uint32_t val = (param[0] << 24) | (param[1] << 16) | (param[2] << 8) | param[3];
            printf("%-23u", val);
        } else {
            printf("%-23s", "(多字节数据)");
        }
        printf(" │ 0x%08lX       │\n", (unsigned long)param);
    }

    if (param_count > 10) {
        printf("│ ...         │ ...        │ ...          │ (还有%u个参数)          │ ...              │\n", param_count - 10);
    }
    printf("└─────────────┴────────────┴──────────────┴─────────────────────────┴──────────────────┘\n");
}

/* M型包数据表格 */
static void print_m_data_table(const uint8_t *data, uint16_t data_len) {
    printf("M型包 - 带延迟的信息参数 (Message with delay)\n");
    printf("┌─────────────┬────────────┬────────────┬────────────┬─────────────────────┬──────────────────┐\n");
    printf("│ Param ID    │ Offset     │ Length     │ Dataset    │ 数据内容 (前16字节)  │ 内存地址         │\n");
    printf("├─────────────┼────────────┼────────────┼────────────┼─────────────────────┼──────────────────┤\n");

    uint16_t pos = 0;
    int count = 0;
    while (pos + 6 <= data_len && count < 5) {  // 最多显示5个参数
        uint16_t param_id = (data[pos] << 8) | data[pos + 1];
        uint16_t offset = (data[pos + 2] << 8) | data[pos + 3];
        uint16_t length = (data[pos + 4] << 8) | data[pos + 5];

        printf("│ 0x%04X      │ %-10u │ %-10u │ %-10u │ ", param_id, offset, length, length);

        // 显示数据内容 (前16字节)
        int display_len = (length > 16) ? 16 : length;
        if (pos + 6 + length <= data_len) {
            for (int i = 0; i < display_len; i++) {
                printf("%02X ", data[pos + 6 + i]);
            }
            if (length > 16) printf("...");
        } else {
            printf("数据越界");
        }
        printf(" │ 0x%08lX       │\n", (unsigned long)&data[pos]);

        pos += 6 + length;
        if (length & 1) pos++;  // padding
        count++;
    }

    printf("└─────────────┴────────────┴────────────┴────────────┴─────────────────────┴──────────────────┘\n");
}

/* Q型包数据表格 */
static void print_q_data_table(const uint8_t *data, uint16_t data_len) {
    printf("Q型包 - 不带延迟的信息参数 (Message without delay)\n");
    printf("┌─────────────┬────────────┬────────────┬─────────────────────────────┬──────────────────┐\n");
    printf("│ Param ID    │ Length     │ Dataset    │ 数据内容 (前16字节)          │ 内存地址         │\n");
    printf("├─────────────┼────────────┼────────────┼─────────────────────────────┼──────────────────┤\n");

    uint16_t pos = 0;
    int count = 0;
    while (pos + 4 <= data_len && count < 5) {
        uint16_t param_id = (data[pos] << 8) | data[pos + 1];
        uint16_t length = (data[pos + 2] << 8) | data[pos + 3];

        printf("│ 0x%04X      │ %-10u │ %-10u │ ", param_id, length, length);

        int display_len = (length > 16) ? 16 : length;
        if (pos + 4 + length <= data_len) {
            for (int i = 0; i < display_len; i++) {
                printf("%02X ", data[pos + 4 + i]);
            }
            if (length > 16) printf("...");
        } else {
            printf("数据越界");
        }
        printf(" │ 0x%08lX       │\n", (unsigned long)&data[pos]);

        pos += 4 + length;
        if (length & 1) pos++;
        count++;
    }

    printf("└─────────────┴────────────┴────────────┴─────────────────────────────┴──────────────────┘\n");
}

/* D型包数据表格 */
static void print_d_data_table(const uint8_t *data, uint16_t data_len, uint8_t key_status) {
    uint8_t param_bytes = (key_status & 0x07) * 2;
    uint16_t block_size = 4 + param_bytes;
    uint16_t param_count = data_len / block_size;

    printf("D型包 - 带延迟的标准参数 (Standard with delay)\n");
    printf("┌─────────────┬────────────┬─────────────────────────────────────────┬──────────────────┐\n");
    printf("│ Param ID    │ Delay      │ Data Word(s) (%u 字节)                   │ 内存地址         │\n", param_bytes);
    printf("├─────────────┼────────────┼─────────────────────────────────────────┼──────────────────┤\n");

    for (uint16_t i = 0; i < param_count && i < 8; i++) {
        const uint8_t *param = &data[i * block_size];
        uint16_t param_id = (param[0] << 8) | param[1];
        uint16_t delay = (param[2] << 8) | param[3];

        printf("│ 0x%04X      │ %-10u │ ", param_id, delay);
        for (int j = 0; j < param_bytes; j++) {
            printf("%02X ", param[4 + j]);
        }
        printf(" │ 0x%08lX       │\n", (unsigned long)param);
    }

    printf("└─────────────┴────────────┴─────────────────────────────────────────┴──────────────────┘\n");
}

/* N型包数据表格 */
static void print_n_data_table(const uint8_t *data, uint16_t data_len, uint8_t key_status) {
    uint8_t param_bytes = (key_status & 0x07) * 2;
    uint16_t block_size = 2 + param_bytes;
    uint16_t param_count = data_len / block_size;

    printf("N型包 - 不带延迟的标准参数 (Standard without delay)\n");
    printf("┌─────────────┬─────────────────────────────────────────────────┬──────────────────┐\n");
    printf("│ Param ID    │ Data Word(s) (%u 字节)                          │ 内存地址         │\n", param_bytes);
    printf("├─────────────┼─────────────────────────────────────────────────┼──────────────────┤\n");

    for (uint16_t i = 0; i < param_count && i < 10; i++) {
        const uint8_t *param = &data[i * block_size];
        uint16_t param_id = (param[0] << 8) | param[1];

        printf("│ 0x%04X      │ ", param_id);
        for (int j = 0; j < param_bytes; j++) {
            printf("%02X ", param[2 + j]);
        }
        printf(" │ 0x%08lX       │\n", (unsigned long)param);
    }

    printf("└─────────────┴─────────────────────────────────────────────────┴──────────────────┘\n");

}

/**
 * 以表格形式打印IENA包头结构
 */
void iena_print_header_table(const uint8_t *packet_data) {
    if (!packet_data) return;

    // 解析包头字段
    uint16_t iena_key = (packet_data[0] << 8) | packet_data[1];
    uint16_t size = (packet_data[2] << 8) | packet_data[3];
    uint64_t timestamp = 0;
    for(int i = 0; i < 6; i++) {
        timestamp = (timestamp << 8) | packet_data[4 + i];
    }
    uint8_t key_status = packet_data[10];
    uint8_t status = packet_data[11];
    uint16_t seq_num = (packet_data[12] << 8) | packet_data[13];

    printf("\n=== IENA 通用包头 (Universal Header) ===\n");
    printf("┌─────────────────┬────────────┬──────────────┬─────────────────────────┬──────────────────┐\n");
    printf("│ 字段 (Field)    │ 长度       │ 值 (十六进制) │ 值 (十进制/描述)        │ 内存地址         │\n");
    printf("├─────────────────┼────────────┼──────────────┼─────────────────────────┼──────────────────┤\n");
    printf("│ IENA Key        │ 2 字节     │ 0x%04X       │ %u                      │ 0x%08lX       │\n",
           iena_key, iena_key, (unsigned long)&packet_data[0]);
    printf("│ Size            │ 2 字节     │ 0x%04X       │ %u 字节                 │ 0x%08lX       │\n",
           size, size, (unsigned long)&packet_data[2]);
    printf("│ Time            │ 6 字节     │ 0x%012llX   │ %llu 微秒               │ 0x%08lX       │\n",
           (unsigned long long)timestamp, (unsigned long long)timestamp, (unsigned long)&packet_data[4]);
    printf("│ Key Status      │ 1 字节     │ 0x%02X         │ ", key_status);

    // 解析Key Status
    if (key_status & 0x80) printf("P型包");
    else if ((key_status & 0x30) == 0x30) printf("M型包");
    else if ((key_status & 0x30) == 0x20) printf("Q型包");
    else if ((key_status & 0x30) == 0x10) printf("D型包");
    else printf("N型包");

    if ((key_status & 0x80) || ((key_status & 0x20) == 0)) {
        printf(", %u字长度", key_status & 0x07);
    }
    printf("      │ 0x%08lX       │\n", (unsigned long)&packet_data[10]);

    printf("│ Status          │ 1 字节     │ 0x%02X         │ ", status);
    if (status == 0) printf("所有状态正常");
    else {
        if (status & 0x01) printf("延迟异常 ");
        if (status & 0x02) printf("溢出 ");
        if (status & 0x08) printf("GPS失锁 ");
        if (status & 0x10) printf("本地失锁 ");
    }
    printf("        │ 0x%08lX       │\n", (unsigned long)&packet_data[11]);

    printf("│ SEQ NUM         │ 2 字节     │ 0x%04X       │ %u                      │ 0x%08lX       │\n",
           seq_num, seq_num, (unsigned long)&packet_data[12]);
    printf("└─────────────────┴────────────┴──────────────┴─────────────────────────┴──────────────────┘\n");
}

/**
 * 以表格形式打印IENA包的完整结构
 */
void iena_print_packet_table(const uint8_t *packet_data, uint16_t packet_size) {
    if (!packet_data || packet_size < 16) {
        printf("错误: 无效的IENA包\n");
        return;
    }

    uint8_t key_status = packet_data[10];
    uint16_t data_len = packet_size - 16;
    uint16_t end_field = (packet_data[packet_size-2] << 8) | packet_data[packet_size-1];

    // 打印包头表格
    iena_print_header_table(packet_data);

    // 打印数据体表格
    printf("\n=== 数据体结构 ===\n");
    if (data_len == 0) {
        printf("数据体为空\n");
    } else {
        const uint8_t *data_start = packet_data + 14;

        if (key_status & 0x80) {
            // P型包数据表格
            print_p_data_table(data_start, data_len, key_status);
        } else if ((key_status & 0x30) == 0x30) {
            // M型包数据表格
            print_m_data_table(data_start, data_len);
        } else if ((key_status & 0x30) == 0x20) {
            // Q型包数据表格
            print_q_data_table(data_start, data_len);
        } else if ((key_status & 0x30) == 0x10) {
            // D型包数据表格
            print_d_data_table(data_start, data_len, key_status);
        } else {
            // N型包数据表格
            print_n_data_table(data_start, data_len, key_status);
        }
    }

    // 打印包尾表格
    printf("\n=== IENA 通用包尾 (Universal Trailer) ===\n");
    printf("┌─────────────────┬────────────┬──────────────┬─────────────────────────┬──────────────────┐\n");
    printf("│ 字段 (Field)    │ 长度       │ 值 (十六进制) │ 状态                    │ 内存地址         │\n");
    printf("├─────────────────┼────────────┼──────────────┼─────────────────────────┼──────────────────┤\n");
    printf("│ End Field       │ 2 字节     │ 0x%04X       │ %s                      │ 0x%08lX       │\n",
           end_field, (end_field == 0xDEAD) ? "正确 ✓" : "错误 ✗",
           (unsigned long)&packet_data[packet_size-2]);
    printf("└─────────────────┴────────────┴──────────────┴─────────────────────────┴──────────────────┘\n");
}